#include "pins_arduino.h"
#include <LiquidCrystal.h>
//#include "UtilityController.h"
#include <SdFat.h>
//#include <math.h>
#include <SPI.h>
#include <DigitalPin.h>

#include "commands.h"
#include "ControllerPins.h"
#include "intelHEX.h"
#include "ICSP.h"

#define COMMANDBUFFER_SIZE 1024 // buffer for incoming commands to the controller
// command buffer needs to be large enough to fit the page buffer, line buffer, and databuffer ( (line buffer - 5)/2 bytes )
// the Page and Line buffers are for ICSP
#define PAGE_BUFFER_SIZE 512 // buffer for decoded HEX values to be written to the chip
#define LINE_BUFFER_SIZE 261 // buffer for reading from the .HEX file, maximum HEX file line length is 261 characters

volatile uint8_t lastError = 0;
volatile uint8_t commandBuffer [COMMANDBUFFER_SIZE];
volatile uint8_t outstandingCommands = 0;
volatile uint16_t commandBufferIndex = 0; // next buffer position to write new command byte to
uint16_t commandExeIndex = COMMANDBUFFER_SIZE-1;    // last buffer position to be processed
uint8_t currentScreen = 0;


uint8_t fontLoaded = 0;
char currentLogFile[13]="";

// big LCD font by by Michael Pilcher 2/9/2010
byte LT[8] ={
  B00111,
  B01111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111};
byte UB[8] ={
  B11111,
  B11111,
  B11111,
  B00000,
  B00000,
  B00000,
  B00000,
  B00000};
byte RT[8] ={
  B11100,
  B11110,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111};
byte LL[8] ={
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B01111,
  B00111};
byte LB[8] ={
  B00000,
  B00000,
  B00000,
  B00000,
  B00000,
  B11111,
  B11111,
  B11111};
byte LR[8] ={
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11110,
  B11100};
byte MB[8] ={
  B11111,
  B11111,
  B11111,
  B00000,
  B00000,
  B00000,
  B11111,
  B11111};

LiquidCrystal * lcd[2];
LiquidCrystal screen1(A0, A1, A2, A3, A4, A5);
LiquidCrystal screen2(A0, 3, A2, A3, A4, A5);

Sd2Card sdCard;
SdFat sd(&sdCard);
ofstream logfile;
ifstream readfile;

DigitalPin<RESET> masterResetPin;


void setup ()
{

	// hold down the master controller while we setup
//    masterResetPin.outputMode();
    pinMode(RESET,OUTPUT);
	masterResetPin.low();

	for (int i = 0 ; i < COMMANDBUFFER_SIZE; i++) {
		commandBuffer[i]=0;
	}
	commandExeIndex = COMMANDBUFFER_SIZE-1;

//  Serial.begin (115200);   // debugging

  // TODO: clean up startup display
	lcd[0] = &screen1;
	lcd[1] = &screen2;
	currentScreen = 0;
	screen1.begin(16, 2);
	screen1.clear();
	screen1.print("Starting Up ");
	screen1.print(freeMemory());
	
//	Serial.println("Starting Up");
	screen1.home();


	updateFirmware("RECOVER.HEX");
	/*
	 * 	Serial.println("Type any character to start");
	 *
	 while (!Serial.available() ) {}
	char selector = Serial.read();

	if(selector=='1') {
		updateFirmware("FAKE.HEX");
	} else 	if(selector=='2') {
		updateFirmware("2BLINK.HEX");
	} else 	if(selector=='3') {
			updateFirmware("RAPID.HEX");
		}
*/




//	Serial.println("starting");
//	lcd[1]->begin(16, 2);
//	lcd[1]->print("Starting Up");

  // have to send on master in, *slave out*
  pinMode(MISO, OUTPUT);
  
  // turn on SPI in slave mode
  SPCR |= _BV(SPE);
  
  // turn on interrupts
  SPCR |= _BV(SPIE);

    pinMode(5, OUTPUT);
	digitalWrite(5, HIGH);

	delay(20);
	// bring up the master controller 
	masterResetPin.high();
//	Serial.println(freeMemory());
//  Serial.println("ready");



}  // end of setup

//void
// SPI interrupt routine
ISR (SPI_STC_vect){
	static boolean bufferOverflow = false;
	static boolean escapeSeqFlag = false;
	static uint16_t lastValidCommand = 0;    // index for the most recently received command

	digitalWrite(5,HIGH);
	uint8_t c = SPDR;
	int bufferFree = getBufferFree();

	if(bufferFree<2) { // need to leave room for escaped 0x00s, buffer can advance up to 2 per tick
		bufferOverflow = true;
		lastError = ERR_OVERFLOW;
	}
	  // add to buffer if room
	if (escapeSeqFlag) {// if the previous byte was the start of an escape sequence
		switch (c) {
			case CMD_ESC:  // if 2nd byte is a null, buffer 2 nulls
				if ( !bufferOverflow ) {
					escapeSeqFlag = false;
					commandBuffer[commandBufferIndex] = CMD_ESC;
					advanceBufferIndex(1);
					commandBuffer[commandBufferIndex] = CMD_ESC;
					advanceBufferIndex(1);
				}// if we overflowed the buffer during this command eat the byte
			break;

			case CMD_ENDBUFFERECOMMAND:   // if the 2nd byte is 255, signal the end of the command
				if ( bufferOverflow ) {// if there was an overflow we can now reset for the next command
					escapeSeqFlag = false;
					commandBufferIndex = lastValidCommand;
					advanceBufferIndex(1);
					bufferOverflow = false;
				} else {
					escapeSeqFlag = false;
					commandBuffer[commandBufferIndex] = CMD_ESC;
					advanceBufferIndex(1);
					commandBuffer[commandBufferIndex] = CMD_ENDBUFFERECOMMAND;
					lastValidCommand=commandBufferIndex;
					advanceBufferIndex(1);
					outstandingCommands++;
				}
			break;

			case CMD_ENDINTURRPTCOMMAND:   // signal the end of the interrupt command
				escapeSeqFlag = false;
			break;

			case CMD_GETFREEBUFFER:   // get buffer free command
				commandGetFreeBuffer(bufferFree);
			break;

			case CMD_GETDATA:   //do nothing, generic "get data" flag

			break;

			case CMD_SDINIT:   //initialize the SD Card
				commandSDInit();

			break;

			case CMD_SDSIZE:
				commandSDSize();

			break;

			case CMD_READBLOCK:   //read a block off the SD Card
				commandReadBlock();

			break;

			case CMD_READCID:
				commandReadCID();

			break;

			case CMD_SDTYPE:
				commandSDType();

			break;

			case CMD_READSTART:
				commandReadStart();

			break;

			case CMD_READSTOP:
				commandReadStop();

			break;

			case CMD_READDATA:
				commandReadData();

			break;

			case CMD_WRITEBLOCK:
				commandWriteBlock();

			break;

			case CMD_WRITESTART:
				commandWriteStart();

			break;

			case CMD_WRITESTOP:
				commandWriteStop();

			break;

			case CMD_WRITEDATA:
				commandWriteData();

			break;

			case CMD_SETSCKRATE:
				commandSetSckRate();

			break;

			case CMD_ERASE:
				commandErase();

			break;

			case CMD_ERASESINGLEBLOCKENABLE:
				commandEraseSingleBlockEnable();

			break;

			case CMD_SDERROR:
				commandSDError();

			break;

			case CMD_SDERRORCODE:
				commandSDErrorCode();

			break;

			case CMD_SDERRORDATA:
				commandSDErrorData();

			break;

			case CMD_FILEEXISTS:
				commandFileExists();

			break;

			case CMD_FILESIZE:
				commandFileSize();

			break;

			case CMD_FILECLOSE:
				commandFileClose();

			break;

			case CMD_FILEOPEN:
				commandFileOpen();

			break;

			case CMD_FILESEEK:
				commandFileSeek();

			break;

			case CMD_FILEREADLINE:
				commandFileReadline();

			break;

			case CMD_GETERROR:
				commandGetError();

			break;

			default:
				//error
			break;
		}
	} else {// if we're not in the middle of an escape sequence
		if (c == CMD_ESC) {// check to see if this byte is the beginning of an escape sequence
			escapeSeqFlag = true;
		} else {// if not add the byte to the command buffer
			if ( !bufferOverflow ) {
				commandBuffer[commandBufferIndex] = c;
				advanceBufferIndex(1);
			} else {// if we overflowed the buffer during this command eat the byte
				lastError = ERR_OVERFLOW;
			}
		}
	}

	digitalWrite(5,LOW);
	
}

// main loop - wait for flag set in interrupt routine
void loop (void)
{
	static boolean unknownCommand = false; 

	if(unknownCommand){
		bool escTest = false;
		while (getBufferFree() < COMMANDBUFFER_SIZE ) {
			escTest = advanceExeIndex(1);
			if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
					unknownCommand=false;
					break;
			}
		}
	}
	if (outstandingCommands>0) {
		advanceExeIndex(1);

		switch (commandBuffer [commandExeIndex]) {
			case CMD_CLEAR:
				commandClear();
				outstandingCommands--;
			break;
			
			case CMD_PRINTNUMBER:
				commandPrintNumber();
				outstandingCommands--;
			break;

			case CMD_PRINT:
				commandPrint();
				outstandingCommands--;
			break;

			case CMD_HOME:
				commandHome();
				outstandingCommands--;
			break;

			case CMD_CURSOR:
				commandCursor();
				outstandingCommands--;
			break;
			case CMD_WRITE:
				commandWrite();
				outstandingCommands--;
			break;

			case CMD_CURSORMODE:
				commandCursorMode();
				outstandingCommands--;
			break;

			case CMD_BLINKMODE:
				commandBlinkMode();
				outstandingCommands--;
			break;

			case CMD_DISPLAYMODE:
				commandDisplayMode();
				outstandingCommands--;
			break;

			case CMD_SCROLL:
				commandScroll();
				outstandingCommands--;
			break;

			case CMD_AUTOSCROLL:
				commandAutoScroll();
				outstandingCommands--;
			break;
			
			case CMD_TEXTDIRECTION:
				commandTextDirection();
				outstandingCommands--;
			break;
			
			case CMD_CUSTOMCHAR:
				commandCustomChar();
				outstandingCommands--;
			break;
			
			case CMD_SETUP:
				commandSetup();
				outstandingCommands--;
			break;

			case CMD_RENDERBIGNUMBER:
				commandRenderBigNumber();
				outstandingCommands--;
			break;

			case CMD_CLEARBIGNUMBER:
				commandClearBigNumber();
				outstandingCommands--;
			break;

			case CMD_SETSCREEN:
				commandSetScreen();
				outstandingCommands--;
			break;

			case CMD_LOGOPEN:
				commandOpenLog();
				outstandingCommands--;
			break;

			case CMD_LOGNEW:
				commandNewLog();
				outstandingCommands--;
			break;

			case CMD_LOGCLOSE:
				commandCloseLog();
				outstandingCommands--;
			break;

			case CMD_LOGCLEAR:
				commandClearLog();
				outstandingCommands--;
			break;

			case CMD_LOGWRITE:
				commandLogWrite();
				outstandingCommands--;
			break;

			default: 
				//unknown command, flag it for removal
				unknownCommand = true;
				outstandingCommands--;
			break;
		}

		//end
	}  else {
		// no commands to process
	}    
}  // end of loop

inline void commandClear(){
	bool escTest = false;
	escTest = advanceExeIndex(1);
	if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
		lcd[currentScreen]->clear();
	}
}

inline void commandPrintNumber(){
	bool escTest = false;
	uint8_t base, hB, lB;
	advanceExeIndex(1);
	base = commandBuffer [commandExeIndex];
	advanceExeIndex(1);
	hB = commandBuffer [commandExeIndex];
	advanceExeIndex(1);
	lB = commandBuffer [commandExeIndex];

	escTest = advanceExeIndex(1);
	if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
		lcd[currentScreen]->print( word(hB,lB) , base);
	}
}

//TODO convert writes to true Print
inline void commandPrint(){
	uint8_t stringLength = 0;
	advanceExeIndex(1);
	stringLength = commandBuffer [commandExeIndex];

	for(int i = 0; i<stringLength; i++){
		advanceExeIndex(1);
		lcd[currentScreen]->write (commandBuffer [commandExeIndex]);
	}

	advanceExeIndex(2);
}

inline void commandHome(){
	bool escTest = false;
	escTest = advanceExeIndex(1);
	if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
		lcd[currentScreen]->home();
	}

}


inline void commandCursor(){
	bool escTest = false;
	uint8_t row, column;
	advanceExeIndex(1);
	column = commandBuffer [commandExeIndex];
	advanceExeIndex(1);
	row = commandBuffer [commandExeIndex];

	escTest = advanceExeIndex(1);
	if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
		lcd[currentScreen]->setCursor(column,row);
	}
}

inline void commandWrite(){
	bool escTest = false;
	uint8_t character;
	advanceExeIndex(1);
	character = commandBuffer[commandExeIndex];

	escTest = advanceExeIndex(1);
	if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
		lcd[currentScreen]->write (character);
	}
}

inline void commandCursorMode(){
	bool escTest = false;
	uint8_t mode;
	advanceExeIndex(1);
	mode = commandBuffer [commandExeIndex];

	escTest = advanceExeIndex(1);
	if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
		switch (mode) {
			case 0:
				lcd[currentScreen]->noCursor();
			break;

			case 1:
				lcd[currentScreen]->cursor();
			break;

			default:
				// invalid cursor mode
			break;
		}
	}
}

inline void commandBlinkMode(){
	bool escTest = false;
	uint8_t mode;
	advanceExeIndex(1);
	mode = commandBuffer [commandExeIndex];

	escTest = advanceExeIndex(1);
	if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
		switch (mode) {
			case 0:
				lcd[currentScreen]->noBlink();
			break;

			case 1:
				lcd[currentScreen]->blink();
			break;

			default:
				// invalid blink mode
			break;
		}
	}
}

inline void commandDisplayMode(){
	bool escTest = false;
	uint8_t mode;
	advanceExeIndex(1);
	mode = commandBuffer [commandExeIndex];

	escTest = advanceExeIndex(1);
	if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
		switch (mode) {
			case 0:
				lcd[currentScreen]->noDisplay();
			break;

			case 1:
				lcd[currentScreen]->display();
			break;

			default:
				// invalid display mode
			break;
		}
	}
}

inline void commandScroll(){
	bool escTest = false;
	uint8_t direction;
	advanceExeIndex(1);
	direction = commandBuffer [commandExeIndex];

	escTest = advanceExeIndex(1);
	if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
		switch (direction) {
			case 0:
				lcd[currentScreen]->scrollDisplayLeft();
			break;

			case 1:
				lcd[currentScreen]->scrollDisplayRight();
			break;

			default:
				// invalid direction
			break;
		}
	}
}

inline void commandAutoScroll(){
	bool escTest = false;
	uint8_t mode;
	advanceExeIndex(1);
	mode = commandBuffer [commandExeIndex];

	escTest = advanceExeIndex(1);
	if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
		switch (mode) {
			case 0:
				lcd[currentScreen]->noAutoscroll();
			break;

			case 1:
				lcd[currentScreen]->autoscroll();
			break;

			default:
				// invalid mode
			break;
		}
	}
}

inline void commandTextDirection(){
	bool escTest = false;
	uint8_t mode;
	advanceExeIndex(1);
	mode = commandBuffer [commandExeIndex];

	escTest = advanceExeIndex(1);
	if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
		switch (mode) {
			case 0:
				lcd[currentScreen]->leftToRight();
			break;

			case 1:
				lcd[currentScreen]->rightToLeft();
			break;

			default:
				// invalid cursor mode
			break;
		}
	}
}

inline void commandCustomChar(){
	bool escTest = false;
	uint8_t index = 0;
	byte character[8];

	advanceExeIndex(1);
	index = commandBuffer [commandExeIndex];

	for(int i = 0; i<8; i++){
		advanceExeIndex(1);
		character[i] = commandBuffer [commandExeIndex];
	}
	escTest = advanceExeIndex(1);
	if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
		lcd[currentScreen]->createChar(index,character);
		fontLoaded = 0;
	}
}

inline void commandSetup(){
	bool escTest = false;
	uint8_t row, column;
	advanceExeIndex(1);
	column = commandBuffer [commandExeIndex];
	advanceExeIndex(1);
	row = commandBuffer [commandExeIndex];

	escTest = advanceExeIndex(1);
	if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
		lcd[currentScreen]->begin(column, row);
	}
}

inline void commandSetScreen(){
	bool escTest = false;
	uint8_t newScreen = 0;
	advanceExeIndex(1);
	newScreen = commandBuffer [commandExeIndex];
	if (newScreen > 1 ) newScreen = 0;

	escTest = advanceExeIndex(1);
	if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
		currentScreen = newScreen;
	}
}

inline void commandRenderBigNumber(){
	bool escTest = false;
	uint8_t number, column;
	advanceExeIndex(1);
	number = commandBuffer [commandExeIndex];
	advanceExeIndex(1);
	column = commandBuffer [commandExeIndex];

	escTest = advanceExeIndex(1);
	if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
		//TODO: add column setting
		if(fontLoaded == 0) {
			setupBigFont();
		}
		bigPrint(number);
	}
}

inline void commandClearBigNumber(){
	bool escTest = false;
	uint8_t column;
	advanceExeIndex(1);
	column = commandBuffer [commandExeIndex];

	escTest = advanceExeIndex(1);
	if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
		clearBigNumber(column);
	}
}

inline void setupBigFont(){
	lcd[currentScreen]->createChar(0,LT);
	lcd[currentScreen]->createChar(1,UB);
	lcd[currentScreen]->createChar(2,RT);
	lcd[currentScreen]->createChar(3,LL);
	lcd[currentScreen]->createChar(4,LB);
	lcd[currentScreen]->createChar(5,LR);
	lcd[currentScreen]->createChar(6,MB);
	fontLoaded = 1;
}

inline void bigPrint(uint8_t number)
{ // uses segments to build the number 
	clearBigNumber(1);
	renderBigNumber(number%10,1);
	clearBigNumber(0);
	renderBigNumber((number%100)/10,0);
}

inline void renderBigNumber(uint8_t number,uint8_t row)
{ 
  switch (number) {
    case 0:
  lcd[currentScreen]->setCursor(row*4,0);
  lcd[currentScreen]->print("\0\1\2");
  lcd[currentScreen]->setCursor(row*4, 1); 
  lcd[currentScreen]->print("\3\4\5");
  break;
    case 1:
  lcd[currentScreen]->setCursor(row*4,0);
  lcd[currentScreen]->print("\1\2");
  lcd[currentScreen]->setCursor(row*4,1);
  lcd[currentScreen]->print("\4\xff\4");

  break;
    case 2:
  lcd[currentScreen]->setCursor(row*4,0);
  lcd[currentScreen]->print("\6\6\2");
  lcd[currentScreen]->setCursor(row*4, 1);
  lcd[currentScreen]->print("\3\4\4");
  break;
    case 3:
  lcd[currentScreen]->setCursor(row*4,0);
  lcd[currentScreen]->print("\6\6\2");
  lcd[currentScreen]->setCursor(row*4, 1);
  lcd[currentScreen]->print("\4\4\5");
  break;
    case 4:
  lcd[currentScreen]->setCursor(row*4,0);
  lcd[currentScreen]->print("\3\4\xff");
  lcd[currentScreen]->setCursor(row*4+2, 1);
  lcd[currentScreen]->write(255);
  break;
    case 5:
  lcd[currentScreen]->setCursor(row*4,0);
  lcd[currentScreen]->print("\3\6\6");
  lcd[currentScreen]->setCursor(row*4, 1);
  lcd[currentScreen]->print("\4\4\5");
  break;
    case 6:
  lcd[currentScreen]->setCursor(row*4,0);
  lcd[currentScreen]->print("\0\6\6");
  lcd[currentScreen]->setCursor(row*4, 1);
  lcd[currentScreen]->print("\3\4\5");
  break;
    case 7:
  lcd[currentScreen]->setCursor(row*4,0);
  lcd[currentScreen]->print("\1\1\2");
  lcd[currentScreen]->setCursor(row*4+2, 1);
  lcd[currentScreen]->write(255);
      break;
    case 8:
  lcd[currentScreen]->setCursor(row*4,0);
  lcd[currentScreen]->print("\0\6\2");
  lcd[currentScreen]->setCursor(row*4, 1);
  lcd[currentScreen]->print("\3\4\5");

  break;
    case 9:
  lcd[currentScreen]->setCursor(row*4,0);
  lcd[currentScreen]->print("\0\6\2");

  lcd[currentScreen]->setCursor(row*4+2, 1);
  lcd[currentScreen]->write(255);
  break;
    default: 
      // if nothing else matches, do the default
  break;
  }
}

inline void clearBigNumber(uint8_t row)
{ // clears the area the custom number is displayed in
 lcd[currentScreen]->setCursor(row*4,0);
 lcd[currentScreen]->print("   ");
 lcd[currentScreen]->setCursor(row*4,1);
 lcd[currentScreen]->print("   ");
}

void advanceBufferIndex(int amount){
	commandBufferIndex += amount;
	while (commandBufferIndex >= sizeof commandBuffer) {
		commandBufferIndex -= sizeof commandBuffer;
	} 
}

inline void commandSDInit(){
	bool retVal = false;
	uint8_t setSpeed,select;
	while(!(SPSR & (1<<SPIF))); // wait for next byte
	//Serial.println("a");
	setSpeed = SPDR;
	while(!(SPSR & (1<<SPIF)));
	//Serial.println("b");
	select = SPDR;

	SPDR = 0xFF; // set MISO high for busy

	retVal = sdCard.init(setSpeed, SDSELECT);  //ignore the CS byte

	SPDR = 0x00; // bring MISO low
	SPDR = retVal;

}

inline void commandSDSize(){

	SPDR = 0xFF; // set MISO high for busy
	uint32_t retVal = 0;
	retVal = sdCard.cardSize();
	cli(); // disable interrupts because SDCard turns them on

	SPDR = 0x00; // bring MISO low
	while(!(SPSR & (1<<SPIF))); // wait for next byte
	SPDR = retVal>>24;
	while(!(SPSR & (1<<SPIF))); // wait for next byte
	SPDR = retVal>>16;
	while(!(SPSR & (1<<SPIF))); // wait for next byte
	SPDR = retVal>>8;
	while(!(SPSR & (1<<SPIF))); // wait for next byte
	SPDR = retVal;

}

inline void commandReadBlock(){
	uint32_t address = 0;
	uint8_t readBuffer[512];
	bool retVal = false;
	int i = 0;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	address = SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte

	address = (address<<8) | SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	address = (address<<8) | SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	address = (address<<8) | SPDR;

	SPDR = 0xFF; // set MISO high for busy

	retVal = sdCard.readBlock(address, readBuffer);
	cli(); // turn interrupts back off after sdCard

	SPDR = 0x00; // bring MISO low
	while(!(SPSR & (1<<SPIF))); // wait for next byte
	SPDR = retVal;

	if(retVal){
		for ( i = 0; i < 512; ++i) {
			while(!(SPSR & (1<<SPIF))); // wait for next byte
			SPDR = readBuffer[i];
		}
	}
}

inline void commandReadCID(){
	cid_t tempCid;
	bool retVal = false;

	SPDR = 0xFF; // set MISO high for busy

	retVal = sdCard.readCID(&tempCid);
	cli(); // turn interrupts back off after sdCard

	SPDR = 0x00; // bring MISO low
	while(!(SPSR & (1<<SPIF))); // wait for next byte
	SPDR = retVal;

	if(retVal){
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.mid;
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.oid[0];
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.oid[1];
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.pnm[0];
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.pnm[1];
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.pnm[2];
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.pnm[3];
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.pnm[4];
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.prv_m;
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.prv_n;
//
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.psn>>24;
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.psn>>16;
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.psn>>8;
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.psn;
//
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.mdt_year_high;
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.reserved;
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.mdt_month;
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.mdt_year_low;
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.always1;
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = tempCid.crc;
	}
}

inline void commandSDType(){

	SPDR = 0xFF; // set MISO high for busy
	uint8_t retVal = 0;
	retVal = sdCard.type();
	cli(); // disable interrupts because SDCard turns them on

	SPDR = retVal; // return value is less than 127 so no need to do a continue byte

}

inline void commandReadStart(){
	uint32_t address = 0;
	bool retVal = false;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	address = SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte

	address = (address<<8) | SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	address = (address<<8) | SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	address = (address<<8) | SPDR;

	SPDR = 0xFF; // set MISO high for busy

	retVal = sdCard.readStart(address);
	cli(); // turn interrupts back off after sdCard

	SPDR = retVal; // return value is less than 127 so no need to do a continue byte

}
inline void commandReadStop(){
	bool retVal = false;

	SPDR = 0xFF; // set MISO high for busy

	retVal = sdCard.readStop();
	cli(); // turn interrupts back off after sdCard

	SPDR = retVal; // return value is less than 127 so no need to do a continue byte

}
inline void commandReadData(){
	uint8_t readBuffer[512];
	bool retVal = false;
	int i = 0;

	SPDR = 0xFF; // set MISO high for busy

	retVal = sdCard.readData(readBuffer);
	cli(); // turn interrupts back off after sdCard

	SPDR = retVal;// return value is less than 127 so no need to do a continue byte

	if(retVal){
		for ( i = 0; i < 512; ++i) {
			while(!(SPSR & (1<<SPIF))); // wait for next byte
			SPDR = readBuffer[i];
		}
	}
}

inline void commandWriteBlock(){
	uint32_t address = 0;
	uint8_t writeBuffer[512];
	bool retVal = false;
	int i = 0;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	address = SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte

	address = (address<<8) | SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	address = (address<<8) | SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	address = (address<<8) | SPDR;

	for ( i = 0; i < 512; ++i) {
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		writeBuffer[i] = SPDR;
	}

	SPDR = 0xFF; // set MISO high for busy

	retVal = sdCard.writeBlock(address, writeBuffer);
	cli(); // turn interrupts back off after sdCard

	SPDR = retVal;// return value is less than 127 so no need to do a continue byte

}

inline void commandWriteStart(){
	uint32_t address = 0;
	uint32_t eraseCount = 0;
	bool retVal = false;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	address = SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte

	address = (address<<8) | SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	address = (address<<8) | SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	address = (address<<8) | SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	eraseCount = SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte

	eraseCount = (eraseCount<<8) | SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	eraseCount = (eraseCount<<8) | SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	eraseCount = (eraseCount<<8) | SPDR;

	SPDR = 0xFF; // set MISO high for busy

	retVal = sdCard.writeStart(address, eraseCount);
	cli(); // turn interrupts back off after sdCard

	SPDR = retVal; // return value is less than 127 so no need to do a continue byte

}

inline void commandWriteStop(){
	bool retVal = false;

	SPDR = 0xFF; // set MISO high for busy

	retVal = sdCard.writeStop();
	cli(); // turn interrupts back off after sdCard

	SPDR = retVal; // return value is less than 127 so no need to do a continue byte

}
inline void commandWriteData(){
	uint8_t writeBuffer[512];
	bool retVal = false;
	int i = 0;
	for ( i = 0; i < 512; ++i) {
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		writeBuffer[i] = SPDR;
	}
	SPDR = 0xFF; // set MISO high for busy

	retVal = sdCard.writeData(writeBuffer);
	cli(); // turn interrupts back off after sdCard

	SPDR = retVal;// return value is less than 127 so no need to do a continue byte

}

inline void commandSetSckRate(){
	bool retVal = false;
	uint8_t setSpeed;
	while(!(SPSR & (1<<SPIF))); // wait for next byte
	setSpeed = SPDR;
	SPDR = 0xFF; // set MISO high for busy

	retVal = sdCard.setSckRate(setSpeed);  //ignore the CS byte

	SPDR = retVal;

}

inline void commandErase(){
	uint32_t firstBlock = 0;
	uint32_t lastBlock = 0;
	bool retVal = false;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	firstBlock = SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte

	firstBlock = (firstBlock<<8) | SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	firstBlock = (firstBlock<<8) | SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	firstBlock = (firstBlock<<8) | SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	lastBlock = SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte

	lastBlock = (lastBlock<<8) | SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	lastBlock = (lastBlock<<8) | SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	lastBlock = (lastBlock<<8) | SPDR;

	SPDR = 0xFF; // set MISO high for busy

	retVal = sdCard.erase(firstBlock, lastBlock);
	cli(); // turn interrupts back off after sdCard

	SPDR = retVal; // return value is less than 127 so no need to do a continue byte
}

inline void commandEraseSingleBlockEnable(){
	bool retVal = false;
	SPDR = 0xFF; // set MISO high for busy

	retVal = sdCard.eraseSingleBlockEnable();

	SPDR = retVal; // return value is less than 127 so no need to do a continue byte
}

inline void commandSDError(){
	uint8_t code = 0;
	while(!(SPSR & (1<<SPIF))); // wait for next byte
	code = SPDR;

	sdCard.error(code);
}

inline void commandSDErrorCode(){
	uint8_t retVal = 0;
	SPDR = sdCard.errorCode();
}

inline void commandSDErrorData(){
	uint8_t retVal = 0;
	SPDR = sdCard.errorData();
}

inline void commandOpenLog(){
	uint16_t i = 0;
	uint8_t stringLength = 0;

	advanceExeIndex(1);
	stringLength = commandBuffer [commandExeIndex];

	char prefix[stringLength+1];

	for(i = 0; i<stringLength; i++){
		advanceExeIndex(1);
		prefix[i]=commandBuffer [commandExeIndex];
	}
	prefix[i+1]=0x00;

	char buffer[9]="";
	char fileName[13]="";

	  if (!sd.init(SPI_HALF_SPEED, SDSELECT)) sd.initErrorHalt();

	  // create a new file in root, the current working directory

	  for (i = 0; i < (pow(10,(8-stringLength)-1)); i++) {

		itoa (i,buffer,10);
		strcpy(fileName,prefix);
		strcat(fileName,buffer);
		strcat(fileName,".log");
	    if (!sd.exists(fileName)) break;

	  }

	  if(i>0){ // if we found log files open the most recent
		i--;
		itoa (i,buffer,10);
		strcpy(fileName,prefix);
		strcat(fileName,buffer);
		strcat(fileName,".log");
		logfile.open(fileName, ios::out | ios::app);
		if (!logfile.is_open()) ;//error("file.open");

	  }else { // if there are no log files at all create one
		  	logfile.open(fileName, ios::out);
		  	strcpy(currentLogFile,fileName);
		  	if (!logfile.is_open()) ;//error("file.open");
	  }

	advanceExeIndex(2);
}

inline void commandNewLog(){
	uint16_t i = 0;
	uint8_t stringLength = 0;

	advanceExeIndex(1);
	stringLength = commandBuffer [commandExeIndex];

	char prefix[stringLength+1];

	for(i = 0; i<stringLength; i++){
		advanceExeIndex(1);
		prefix[i]=commandBuffer [commandExeIndex];
	}
	prefix[i+1]=0x00;

	char buffer[9]="";
	char fileName[9]="";

	  if (!sd.init(SPI_HALF_SPEED, SDSELECT)) sd.initErrorHalt();

	  // create a new file in root, the current working directory

	  for (i = 0; i < (pow(10,(8-stringLength)-1)); i++) {

		itoa (i,buffer,10);
		strcpy(fileName,prefix);
		strcat(fileName,buffer);
		strcat(fileName,".log");
	    if (!sd.exists(fileName)) break;

	  }

  	logfile.open(fileName, ios::out);
  	strcpy(currentLogFile,fileName);

  	if (!logfile.is_open()) ;//error("file.open");

	advanceExeIndex(2);
}

inline void commandCloseLog(){
	bool escTest = false;
	escTest = advanceExeIndex(1);
	if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
		logfile.close();// close the file
		currentLogFile[0] = 0x00;

	}
}

inline void commandClearLog(){
	bool escTest = false;
	escTest = advanceExeIndex(1);
	if((escTest == true) && (commandBuffer[commandExeIndex] == CMD_ENDBUFFERECOMMAND)) {
		logfile.close();// close the file
	  	logfile.open(currentLogFile, ios::out | ios::trunc); //open new file, truncate
	}
}

inline void commandLogWrite(){
	uint8_t i = 0;
	uint8_t stringLength = 0;

	advanceExeIndex(1);
	stringLength = commandBuffer [commandExeIndex];

	for(i = 0; i<stringLength; i++){
		advanceExeIndex(1);
		logfile<<commandBuffer [commandExeIndex];
	}

	logfile.flush();

	advanceExeIndex(2);
}

inline void commandFileExists(){
	uint8_t length = 0;
	uint8_t i = 0;
	char filename[13]="";

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	length = SPDR;
	for (i = 0; i < length; ++i) {
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		filename[i] = SPDR;
	}
	SPDR = 0xFF; // set MISO high for busy
	filename[i+1]=0x00;
	SPDR = sd.exists(filename);

}

inline void commandFileSize(){
	uint8_t length = 0;
	uint8_t i = 0;
	uint32_t retVal = 0;
	char filename[13]="";

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	length = SPDR;
	for (i = 0; i < length; ++i) {
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		filename[i] = SPDR;
	}
	SPDR = 0xFF; // set MISO high for busy
	filename[i+1]=0x00;

	readfile.open(filename);
	readfile.seekg(0, ios::end);// seek to the end of the file
	retVal = readfile.tellg();
	readfile.close();

	SPDR = 0x00; // reset busy
	while(!(SPSR & (1<<SPIF))); // wait for next byte
	SPDR = retVal>>24;
	while(!(SPSR & (1<<SPIF))); // wait for next byte
	SPDR = retVal>>16;
	while(!(SPSR & (1<<SPIF))); // wait for next byte
	SPDR = retVal>>8;
	while(!(SPSR & (1<<SPIF))); // wait for next byte
	SPDR = retVal;

}

inline void commandFileClose(){
	SPDR = 0xFF; // set MISO high for busy
	readfile.close();
	SPDR = 0x00; // reset busy

}

inline void commandFileOpen(){
	uint8_t length = 0;
	uint8_t i = 0;
	char filename[13]="";

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	length = SPDR;
	for (i = 0; i < length; ++i) {
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		filename[i] = SPDR;
	}
	SPDR = 0xFF; // set MISO high for busy
	filename[i+1]=0x00;

	readfile.open(filename);

	SPDR = 0x00; // reset busy
}

inline void commandFileSeek(){
	uint32_t location = 0;


	while(!(SPSR & (1<<SPIF))); // wait for next byte
	location = SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte

	location = (location<<8) | SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	location = (location<<8) | SPDR;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	location = (location<<8) | SPDR;


	SPDR = 0xFF; // set MISO high for busy


	readfile.seekg(location);


	SPDR = 0x00; // reset busy

}

inline void commandFileReadline(){
	uint8_t maxLength = 0;
	uint8_t readBytes = 0;
	char separator;
	uint8_t *readbuffer;

	while(!(SPSR & (1<<SPIF))); // wait for next byte
	maxLength = SPDR;
	while(!(SPSR & (1<<SPIF))); // wait for next byte
	separator = SPDR;
	SPDR = 0xFF; // set MISO high for busy

	if(getContiguousBufferSize()>=maxLength){

		readbuffer = getContiguousBufferAddress();

		readfile.getline((char*)readbuffer, maxLength, separator);
		readBytes = readfile.gcount();
		if(readfile.fail()){
			lastError = ERR_PARTIALREAD;
			readfile.clear(readfile.rdstate() & ~ios_base::failbit);
		} else if (readfile.eof()){
			lastError = ERR_EOF;
		}
	}


	SPDR = 0x00; // reset busy
	while(!(SPSR & (1<<SPIF))); // wait for next byte
	SPDR = readBytes;

	for (uint8_t i = 0; i < readBytes; ++i) {
		while(!(SPSR & (1<<SPIF))); // wait for next byte
		SPDR = readbuffer[i];
	}
}

inline void commandGetError(){
	SPDR = lastError;
	lastError = ERR_NONE;
}

inline void commandGetFreeBuffer(int bufferFree){
	SPDR = highByte(bufferFree);
	while(!(SPSR & (1<<SPIF))); // wait for next byte
	SPDR = lowByte(bufferFree);
}

bool advanceExeIndex(int amount){
	commandExeIndex += amount;
	while (commandExeIndex >= COMMANDBUFFER_SIZE) {
		commandExeIndex -= COMMANDBUFFER_SIZE;
	} 
	
	if(commandBuffer[commandExeIndex] == CMD_ESC ) { // skip over the escaping null
		commandExeIndex++;
		if (commandExeIndex >= COMMANDBUFFER_SIZE) {
			commandExeIndex = commandExeIndex - COMMANDBUFFER_SIZE;
		} 
		return(true);//return true if we skipped an esc char
	} else {
		return(false);//return false if there was no esc char
	}
}

int getBufferFree(){
	if(commandBufferIndex>commandExeIndex){
		return ( COMMANDBUFFER_SIZE - (commandBufferIndex-commandExeIndex));
	}else{
		return ( commandExeIndex-commandBufferIndex);
	}
}

uint8_t* getContiguousBufferAddress(){
	if(commandBufferIndex>commandExeIndex){
		if( (COMMANDBUFFER_SIZE - commandBufferIndex) > (commandExeIndex)){
			return ( (uint8_t*)&commandBuffer[commandBufferIndex]);
		} else {
			return ( (uint8_t*)&commandBuffer[0]);
		}
	}else{
		return ( (uint8_t*)&commandBuffer[commandBufferIndex]);
	}
}

uint16_t getContiguousBufferSize(){
	if(commandBufferIndex>commandExeIndex){
		if( (COMMANDBUFFER_SIZE - commandBufferIndex) > (commandExeIndex)){
			return (COMMANDBUFFER_SIZE - commandBufferIndex);
		} else {
			return (commandExeIndex);
		}
	}else{
		return (COMMANDBUFFER_SIZE - commandBufferIndex - (COMMANDBUFFER_SIZE-commandExeIndex));
	}
}

inline void updateFirmware(char updateFile[]){

	uint8_t *pageBuffer = (uint8_t*)commandBuffer;
	uint8_t *lineBuffer = (uint8_t*)commandBuffer+PAGE_BUFFER_SIZE;
	uint8_t *dataBuffer = (uint8_t*)commandBuffer+PAGE_BUFFER_SIZE+LINE_BUFFER_SIZE;

	hexCommand packetHeader;

	char filename[13]=""; //for some reason ifstream fails if passed the parameter directly, but works if we copy the string locally
	strcpy(filename,updateFile);

	uint16_t pageOffset = 0;

	uint16_t lineNumber = 1;
	uint16_t chipSignature = 0;
	bool fail = false;

	if (sd.init(SPI_FULL_SPEED, SDSELECT)) {
		if (sd.exists(filename)) {
			ifstream updateFile(filename);
			ofstream updateLog("UPDATE.LOG", ios::out | ios::trunc);
			while  (updateFile.getline((char*)lineBuffer, LINE_BUFFER_SIZE, '\n') || updateFile.gcount()) {
				//check crc for each line
				if(!checkCRC((char*)lineBuffer)){
					fail = true;
					break;
				}
				lineNumber++;
			}

			if(fail){
				updateLog<<"CRC failed on line "<< lineNumber <<endl;
				return;
			} else {
				updateLog<<"CRC checks OK"<<endl;
				updateLog<<"Processed "<<lineNumber<<" lines"<<endl;
			}

			startProgramMode();
			chipSignature = readSignature();
//			Serial.println(chipSignature, HEX);
			updateLog<<"Detected chip signature "<<chipSignature<<endl;

			if(true){ // TODO: chip signature check
				// set programming fuses
				programFuse(FUSE_PROT, 0xFF);
				programFuse(FUSE_LOW, 0xFF);
				programFuse(FUSE_HIGH, 0xD7);
				programFuse(FUSE_EXT, 0xFC);
				updateLog<<"Finished setting prog fuses"<<endl;

				chipErase();
				updateLog<<"Finished full chip erase"<<endl;

				//reparse and program HEX
				updateFile.seekg(0);//reset file to the beginning
				uint16_t byteAddress = 0;

				while  (updateFile.getline((char*)lineBuffer, LINE_BUFFER_SIZE, '\n') || updateFile.gcount()) {
					packetHeader = parseHexCommand(lineBuffer, dataBuffer);
					// copy the current data into the page buffer
					for (uint8_t i = 0; i < packetHeader.byteCount; i++) {
						byteAddress = packetHeader.startAddress+i;

						if (byteAddress >= pageOffset) { // only if address is ahead of the buffer, if it's trying to write to a page we've already flushed discard the data
							if (byteAddress > pageOffset + PAGE_BUFFER_SIZE-1) {
								// need to flush the page
								//Serial.print ("finished page at ");
								//Serial.print(pageOffset, DEC);
								//Serial.print(" next byte is ");
								//Serial.println(byteAddress, DEC);
								if(pageOffset >= 0){

									programPage (pageBuffer, PAGE_BUFFER_SIZE, pageOffset, 64);
								}
								memset(pageBuffer, 0, PAGE_BUFFER_SIZE);
								pageOffset = byteAddress;
							}
							pageBuffer[byteAddress-pageOffset] = dataBuffer[i];
						}
					}
				}
				//flush any remaining buffer to the chip
				programPage (pageBuffer, byteAddress-pageOffset, pageOffset, 64); //if we didn't fill the entire pagebuffer only commit the bytes that we have

				updateLog<<"Finished writing final byte "<<byteAddress<<endl;;


				//restore normal fuses
				programFuse(FUSE_PROT, 0xF3);
				updateLog<<"Finished resetting program fuse"<<endl;
				endProgramMode();
				updateLog<<"Programming complete!"<<endl;
			}

		} else {
			// couldn't find update file
		}

	} else {
		// no card or read failure
	}


}

extern unsigned int __bss_end;
extern unsigned int __heap_start;
extern void *__brkval;

int freeMemory() {
  int free_memory;

  if((int)__brkval == 0)
     free_memory = ((int)&free_memory) - ((int)&__bss_end);
  else
    free_memory = ((int)&free_memory) - ((int)__brkval);
 // return(14);
  return free_memory;//-(6*1024);
}
